import rclpy
from rclpy.node import Node
from turtlesim.msg import Pose

class PoseSubscriber(Node):
    def __init__(self):
        super().__init__('pose_subscriber')
        # 创建订阅者，订阅名为'/turtle1/pose'的topic
        self.subscription = self.create_subscription(
            Pose,  # 消息类型
            '/turtle1/pose',  # topic名称
            self.pose_callback,  # 回调函数
            10)  # 队列大小
        self.subscription  # 防止未使用的变量警告

    def pose_callback(self, msg):
        # 每次接收到消息时，回调函数会被调用
        self.get_logger().info(f'Pose: x={msg.x:.2f}, y={msg.y:.2f}, theta={msg.theta:.2f}, linear_velocity={msg.linear_velocity:.2f}, angular_velocity={msg.angular_velocity:.2f}')

def main(args=None):
    # 初始化ROS 2 Python客户端
    rclpy.init(args=args)
    # 创建订阅者节点
    node = PoseSubscriber()
    # 使用spin()函数保持节点运行，直到用户手动停止
    rclpy.spin(node)
    # 关闭ROS 2 Python客户端
    rclpy.shutdown()

if __name__ == '__main__':
    main()